#!/usr/bin/env python3

import os
import launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # Include the common launch file
    common_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('my_robot_nav'), 'launch', 'common.launch.py')
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }.items()
    )

    # 设置地图文件路径
    # pbstream_path = os.path.join(get_package_share_directory('my_robot_nav'), 'maps', 'map.pbstream')

    # Cartographer SLAM node
    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        arguments=[
            '-configuration_directory', os.path.join(get_package_share_directory('my_robot_nav'), 'config'),
            '-configuration_basename', 'cartographer.lua',
            # '-load_state_filename', pbstream_path
        ],
        parameters=[{
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }],
        remappings=[
            ('odom', '/odometry/filtered'),
            # ('imu', '/imu/data')
            # ('odom', '/odom'),
            ('imu', '/imu')
        ]
    )

    # Cartographer occupancy grid node
    cartographer_occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{
            'use_sim_time': LaunchConfiguration('use_sim_time'),
            'resolution': 0.01
        }]
    )

    # Nav2 bringup
    nav2_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py')
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time'),
            'params_file': os.path.join(get_package_share_directory('my_robot_nav'), 'config', 'nav2_params.yaml')
        }.items()
    )

    return LaunchDescription([
        common_launch,
        cartographer_node,
        cartographer_occupancy_grid_node,
        # nav2_bringup
    ]) 